#include "Processor.h"
#include "DataType.hpp"
#include "angles.h"

#include <opencv2/core/eigen.hpp>

#include <unsupported/Eigen/NonLinearOptimization>


namespace hnurm
{

    void Processor::ProcessArmor(std::vector<Armor> &armors_msg, VisionRecvData &recv_data, TargetInfo &target_msg)
    {
        // 将装甲位置从图像帧转换为世界坐标
        for (auto &armor: armors_msg)
        {
            // 使用转换解算器将装甲位置从图像帧转换为世界坐标
            tsolver->GetTransformation(armor, (float) angles::from_degrees(recv_data.pitch), (float) angles::from_degrees(recv_data.yaw), dt_);
        }

        // 记录当前时间
        auto time = std::chrono::steady_clock::now();

        if (tracker->tracker_state == Tracker::LOST)
        {
            // 如果跟踪器状态为LOST，则使用检测到的装甲初始化跟踪器
            tracker->init(armors_msg);
        }
        else
        {
            // 计算当前时间和上一次记录时间之间的时间差
            dt_ = (double) std::chrono::duration_cast<Us>(time - time_elapsed_).count() / 1e6;
            // 使用检测到的装甲 更新跟踪器
            tracker->update(armors_msg);
        }
        // 更新time_elapsed_变量为当前时间
        time_elapsed_ = time;

        // 获取跟踪器当前状态
        const auto state = tracker->target_state;

        // 将状态值分配给target_msg对象
        target_msg.x        = (float )state(0);
        target_msg.vx       = (float )state(1);
        target_msg.y        = (float )state(2);
        target_msg.vy       = (float )state(3);
        target_msg.z        = (float )state(4);
        target_msg.vz       = (float )state(5);
        target_msg.yaw      = (float )state(6);
        target_msg.w_yaw    = (float )state(7);
        target_msg.radius_1 = (float )state(8);
        target_msg.radius_2 = (float )tracker->another_r;
        target_msg.dz       = (float )tracker->dz;
        if (tracker->tracked_armor.number == "1") {
            target_msg.type = TargetType::hero;
        }else if (tracker->tracked_armor.number == "2"){
            target_msg.type = TargetType::engineer;
        }
        else if (tracker->tracked_armor.number == "3"){
            target_msg.type = TargetType::infantry3;
        }
        else if (tracker->tracked_armor.number == "4"){
            target_msg.type = TargetType::infantry4;
        }
        else if (tracker->tracked_armor.number == "5"){
            target_msg.type = TargetType::infantry5;
        }
        else if (tracker->tracked_armor.number == "outpost"){
            target_msg.type = TargetType::outpost;
        }
        else if (tracker->tracked_armor.number == "guard"){
            target_msg.type = TargetType::guard;
        }
        else if (tracker->tracked_armor.number == "base"){
            target_msg.type = TargetType::base;
        }
        else if (tracker->tracked_armor.number == "negative"){
            target_msg.type = TargetType::none;
        }

#ifdef DEBUG_SHOW
        // 使用转换解算器可视化装甲
         tsolver->ArmorVisualization(*tracker, target_msg);
#endif
    }

    void Processor::ProcessBuff(Energybuff &buff_msg, VisionRecvData &recv_data, TargetInfo &target_msg) {
        // 使用转换解算器将装甲位置从图像帧转换为世界坐标
        tsolver->GetTransformation(buff_msg.targetarmor, (float) angles::from_degrees(recv_data.pitch), (float) angles::from_degrees(recv_data.yaw), dt_);

        // 记录当前时间
        auto time = std::chrono::steady_clock::now();
        // 更新time_elapsed_变量为当前时间
        time_elapsed_ = time;

        if (buff_msg.trackstate == Energybuff::TrackState::LOST)
        {
            // buff_msg.positions.clear();
        }
        else
        {
            if(buff_msg.positions.size()<50) {
                buff_msg.positions.emplace_back(buff_msg.targetarmor.position);
            }
            else {
                buff_msg.positions.erase(buff_msg.positions.begin());
                buff_msg.positions.emplace_back(buff_msg.targetarmor.position);
                 CircleFittingProblem problem(buff_msg.positions);
                // 初始化参数估计
                Eigen::VectorXd x(3);
                x << 0.0, 7.0, 0.5; // 假设圆心为(0,7,0.5)

                Eigen::LevenbergMarquardt<CircleFittingProblem, double> lm(problem);
                lm.parameters.maxfev = 100;
                lm.parameters.ftol = 1.0e-10; // 函数值收敛容差
                lm.parameters.xtol = 1.0e-10; // 参数值收敛容差

                int status = lm.minimize(x);

                // 判断角速度正负
                std::vector<float> angles;
                if(buff_msg.speed==0) {
                    for(const auto& position:buff_msg.positions) {
                        Eigen::VectorXd vec = position - x;
                        float angle = atan2(vec[2], vec[0]);
                        angles.emplace_back(angle);
                    }
                    float a = std::accumulate(angles.begin(), angles.begin() + 50, 0);
                    float b = std::accumulate(angles.begin() + 50, angles.end(), 0);
                    buff_msg.speed = (a>b)?-CV_PI/3:CV_PI;
                }

                 // 将状态值分配给target_msg对象
                target_msg.x_r      = (float )x[0];
                target_msg.y_r      = (float )x[1];
                target_msg.z_r      = (float )x[2];
                target_msg.radius_1 = 0.7;
                target_msg.w_yaw    = -CV_PI/3; // 角速度
                target_msg.if_ready = true;
                
            
            }

            // 将状态值分配给target_msg对象
            target_msg.x        = buff_msg.targetarmor.position(0);
            target_msg.y        = buff_msg.targetarmor.position(1);
            target_msg.z        = buff_msg.targetarmor.position(2);
            target_msg.radius_1 = 0.7;
            if(buff_msg.lasttarget.angle < buff_msg.targetarmor.angle) {
                target_msg.w_yaw = - CV_PI / 3;
            }
            else {
                target_msg.w_yaw = CV_PI / 3;
            }
        }
        tracker->tracked_armors_num = ArmorsNum::ENERGYBUFF_5;
    }

    Processor::Processor(const cv::FileNode &cfg_node)
    {
        dt_ = 0.01;
        cv::FileNode tracker_config_node = cfg_node["tracker"];

        // 初始化EKF
        tracker = std::make_unique<Tracker>(tracker_config_node);

        // EKF
        // xa = x_armor, xc = x_robot_center
        // state: xc, v_xc, yc, v_yc, za, v_za, yaw, v_yaw, r
        // measurement: xa, ya, za, yaw
        // f - 过程函数
        auto f = [this](const Eigen::VectorXd &x) {
            Eigen::VectorXd x_new = x;
            x_new(0) += x(1) * dt_;
            x_new(2) += x(3) * dt_;
            x_new(4) += x(5) * dt_;
            x_new(6) += x(7) * dt_;
            return x_new;
        };

        // J_f - 过程函数的雅可比矩阵
        auto j_f = [this](const Eigen::VectorXd &) {
            Eigen::MatrixXd f(9, 9);
            // clang-format off
        f <<  1,   dt_, 0,   0,   0,   0,   0,   0,   0,
              0,   1,   0,   0,   0,   0,   0,   0,   0,
              0,   0,   1,   dt_, 0,   0,   0,   0,   0,
              0,   0,   0,   1,   0,   0,   0,   0,   0,
              0,   0,   0,   0,   1,   dt_, 0,   0,   0,
              0,   0,   0,   0,   0,   1,   0,   0,   0,
              0,   0,   0,   0,   0,   0,   1,   dt_, 0,
              0,   0,   0,   0,   0,   0,   0,   1,   0,
              0,   0,   0,   0,   0,   0,   0,   0,   1;
            // clang-format on
            return f;
        };

        // h - 观测函数
        auto h = [](const Eigen::VectorXd &x) {
            Eigen::VectorXd z(4);
            double xc = x(0), yc = x(2), yaw = x(6), r = x(8);
            z(0) = xc - r * cos(yaw);       // xa
            z(1) = yc - r * sin(yaw);       // ya
            z(2) = x(4);                 // za
            z(3) = x(6);                 // yaw
            return z;
        };
        // J_h - 观测函数的雅可比矩阵
        auto j_h = [](const Eigen::VectorXd &x) {
            Eigen::MatrixXd h(4, 9);
            double yaw = x(6), r = x(8);
            // clang-format off
        //    xc   v_xc yc   v_yc za   v_za yaw            v_yaw r
        h <<  1,   0,   0,   0,   0,   0,   r*sin(yaw), 0,    -cos(yaw),
              0,   0,   1,   0,   0,   0,   -r*cos(yaw),0,    -sin(yaw),
              0,   0,   0,   0,   1,   0,   0,             0,    0,
              0,   0,   0,   0,   0,   0,   1,             0,    0;
            // clang-format on
            return h;
        };

        tracker_config_node["s2_q_xyz"] >> s2qxyz_;
        tracker_config_node["s2_q_yaw"] >> s2qyaw_;
        tracker_config_node["s2_q_r"] >> s2qr_;

        // update_Q - 过程噪声协方差矩阵
        auto u_q = [this]() {
            Eigen::MatrixXd q(9, 9);
            double t = dt_, x = s2qxyz_, y = s2qyaw_, r = s2qr_;
            double q_x_x = pow(t, 4) / 4 * x, q_x_vx = pow(t, 3) / 2 * x, q_vx_vx = pow(t, 2) * x;
            double q_y_y = pow(t, 4) / 4 * y, q_y_vy = pow(t, 3) / 2 * x, q_vy_vy = pow(t, 2) * y;
            double q_r = pow(t, 4) / 4 * r;
            // clang-format off
        //    xc      v_xc    yc      v_yc    za      v_za    yaw     v_yaw   r
        q <<  q_x_x,  q_x_vx, 0,      0,      0,      0,      0,      0,      0,
              q_x_vx, q_vx_vx,0,      0,      0,      0,      0,      0,      0,
              0,      0,      q_x_x,  q_x_vx, 0,      0,      0,      0,      0,
              0,      0,      q_x_vx, q_vx_vx,0,      0,      0,      0,      0,
              0,      0,      0,      0,      q_x_x,  q_x_vx, 0,      0,      0,
              0,      0,      0,      0,      q_x_vx, q_vx_vx,0,      0,      0,
              0,      0,      0,      0,      0,      0,      q_y_y,  q_y_vy, 0,
              0,      0,      0,      0,      0,      0,      q_y_vy, q_vy_vy,0,
              0,      0,      0,      0,      0,      0,      0,      0,      q_r;
            // clang-format on
            return q;
        };

        tracker_config_node["r_xyz_factor"] >> r_xyz_factor;
        tracker_config_node["r_yaw"] >> r_yaw;

        // update_R - 测量噪声协方差矩阵
        auto u_r = [this](const Eigen::VectorXd &z) {
            Eigen::DiagonalMatrix<double, 4> r;
            double x = r_xyz_factor;
            r.diagonal() << abs(x * z[0]), abs(x * z[1]), abs(x * z[2]), r_yaw;
            return r;
        };

        // P - 误差估计协方差矩阵
        Eigen::DiagonalMatrix<double, 9> p0;
        p0.setIdentity();

        // 初始化EKF
        tracker->ekf = ExtendedKalmanFilter{f, h, j_f, j_h, u_q, u_r, p0};
    }

}// namespace hnurm
